import math
import time
import serial
import re
from datetime import datetime, timedelta

# 定义全局变量
utctime = ''  # UTC时间
lat = ''  # 纬度
ulat = ''  # 纬度方向 (N/S)
lon = ''  # 经度
ulon = ''  # 经度方向 (E/W)
numSv = ''  # 卫星数量
msl = ''  # 海平面高度
cogt = ''  # 真北航向
cogm = ''  # 磁北航向
sog = ''  # 地面速度 (节)
kph = ''  # 地面速度 (公里/小时)
gps_t = 0  # GPS状态标志

# 连接GPS串口
ser = serial.Serial("COM3", 9600, timeout=1)
# ser = serial.Serial("/dev/ttyUSB0", 9600, timeout=1)

if ser.isOpen():
    print("GPS串口已打开! 波特率=9600")
else:
    print("GPS串口打开失败!")


# 定义GPS地图转换类
class GPS_MAP:
    def __init__(self, debug=False):
        self.PI = 3.1415926535897932384626
        self.A = 6378245.0
        self.EE = 0.00669342162296594323

    def gps84_to_gcj02(self, lat, lon):
        # 判断是否超出中国
        if self.outOfChina(lat, lon):
            return [lat, lon]
        dLat = self.transformLat(lon - 105.0, lat - 35.0)
        dLon = self.transformLon(lon - 105.0, lat - 35.0)
        radLat = lat / 180.0 * self.PI
        magic = math.sin(radLat)
        magic = 1 - self.EE * magic * magic
        sqrtMagic = math.sqrt(magic)
        dLat = (dLat * 180.0) / ((self.A * (1 - self.EE)) / (magic * sqrtMagic) * self.PI)
        dLon = (dLon * 180.0) / (self.A / sqrtMagic * math.cos(radLat) * self.PI)
        mgLat = lat + dLat
        mgLon = lon + dLon
        return [mgLat, mgLon]

    def gcj02_to_bd09(self, gg_lat, gg_lon):
        x = gg_lon
        y = gg_lat
        z = math.sqrt(x * x + y * y) + 0.00002 * math.sin(y * self.PI)
        theta = math.atan2(y, x) + 0.000003 * math.cos(x * self.PI)
        bd_lon = z * math.cos(theta) + 0.0065
        bd_lat = z * math.sin(theta) + 0.006
        return [bd_lat, bd_lon]

    def outOfChina(self, lat, lon):
        if (lon < 72.004 or lon > 137.8347):
            return True
        if (lat < 0.8293 or lat > 55.8271):
            return True
        return False

    def transform(self, lat, lon):
        if (self.outOfChina(lat, lon)):
            return [lat, lon]
        dLat = self.transformLat(lon - 105.0, lat - 35.0)
        dLon = self.transformLon(lon - 105.0, lat - 35.0)
        radLat = lat / 180.0 * self.PI
        magic = math.sin(radLat)
        magic = 1 - self.EE * magic * magic
        sqrtMagic = math.sqrt(magic)
        dLat = (dLat * 180.0) / ((self.A * (1 - self.EE)) / (magic * sqrtMagic) * self.PI)
        dLon = (dLon * 180.0) / (self.A / sqrtMagic * math.cos(radLat) * self.PI)
        mgLat = lat + dLat
        mgLon = lon + dLon
        return [mgLat, mgLon]

    def transformLat(self, x, y):
        ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * math.sqrt(abs(x))
        ret += (20.0 * math.sin(6.0 * x * self.PI) + 20.0 * math.sin(2.0 * x * self.PI)) * 2.0 / 3.0
        ret += (20.0 * math.sin(y * self.PI) + 40.0 * math.sin(y / 3.0 * self.PI)) * 2.0 / 3.0
        ret += (160.0 * math.sin(y / 12.0 * self.PI) + 320 * math.sin(y * self.PI / 30.0)) * 2.0 / 3.0
        return ret

    def transformLon(self, x, y):
        ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * math.sqrt(abs(x))
        ret += (20.0 * math.sin(6.0 * x * self.PI) + 20.0 * math.sin(2.0 * x * self.PI)) * 2.0 / 3.0
        ret += (20.0 * math.sin(x * self.PI) + 40.0 * math.sin(x / 3.0 * self.PI)) * 2.0 / 3.0
        ret += (150.0 * math.sin(x / 12.0 * self.PI) + 300.0 * math.sin(x / 30.0 * self.PI)) * 2.0 / 3.0
        return ret


# 将GPS数据转换为度
def Convert_to_degrees(in_data1, in_data2):
    len_data1 = len(in_data1)
    str_data2 = "%05d" % int(in_data2)  # 补齐数据
    temp_data = int(in_data1)
    symbol = 1
    if temp_data < 0:
        symbol = -1
    degree = int(temp_data / 100.0)  # 获取度数部分
    str_decimal = str(in_data1[len_data1 - 2]) + str(in_data1[len_data1 - 1]) + str(str_data2)
    f_degree = int(str_decimal) / 60.0 / 100000.0  # 获取分数部分
    if symbol > 0:
        result = degree + f_degree  # 正向
    else:
        result = degree - f_degree  # 反向
    return result


# 读取GPS数据
def GPS_read():
    global utctime, lat, ulat, lon, ulon, numSv, msl, cogt, cogm, sog, kph, gps_t
    if ser.inWaiting():  # 如果串口中有数据
        if ser.read(1) == b'G':  # 检查是否为GPS数据
            if ser.inWaiting():
                if ser.read(1) == b'N':  # 检查数据是否为GNGGA
                    if ser.inWaiting():
                        choice = ser.read(1)
                        if choice == b'G':  # GGA数据
                            if ser.inWaiting():
                                if ser.read(1) == b'G':  # GGA数据头
                                    if ser.inWaiting():
                                        if ser.read(1) == b'A':  # 有效GPS数据
                                            # 读取GGA数据
                                            GGA = ser.read(70)
                                            GGA_g = re.findall(r"\w+(?=,)|(?<=,)\w+", str(GGA))  # 使用正则解析数据
                                            if len(GGA_g) < 13:
                                                print("未找到GPS数据")
                                                gps_t = 0
                                                return 0
                                            else:
                                                utctime = GGA_g[0]  # UTC时间
                                                lat = "%.8f" % Convert_to_degrees(str(GGA_g[2]), str(GGA_g[3]))  # 纬度
                                                ulat = GGA_g[4]  # 纬度方向 (N/S)
                                                lon = "%.8f" % Convert_to_degrees(str(GGA_g[5]), str(GGA_g[6]))  # 经度
                                                ulon = GGA_g[7]  # 经度方向 (E/W)
                                                numSv = GGA_g[9]  # 卫星数量
                                                msl = GGA_g[12] + '.' + GGA_g[13] + GGA_g[14]  # 海平面高度
                                                gps_t = 1
                                                return 1


# 主循环
try:
    gps_map = GPS_MAP()  # 创建GPS_MAP实例
    while True:
        if GPS_read():  # 如果成功读取GPS数据
            #  UTC 时间转换为北京时间
            utc_time = datetime.strptime(utctime, '%H%M%S')
            beijing_time = utc_time + timedelta(hours=8)
            beijing_time_str = beijing_time.strftime('%H:%M:%S')

            print("*********************")
            print('北京时间: ' + beijing_time_str)
            # print('卫星数量: ' + numSv)
            # print('真北航向: ' + cogt + '°')
            # print('磁北航向: ' + cogm + '°')
            # print('地面速度: ' + sog + '节')
            # print('地面速度: ' + kph + '公里/小时')
            print('海平面高度: ' + msl)

            # print('WGS-84 坐标: 经度: ' + lon + ulon + ' 纬度: ' + lat + ulat)
            print('WGS-84 坐标: 经度: ' + lon + ' 纬度: ' + lat)

            # 转换WGS-84坐标到GCJ-02
            gcj02_coords = gps_map.gps84_to_gcj02(float(lat), float(lon))
            print('GCJ-02 坐标: 经度: ' + str(gcj02_coords[1]) + ' 纬度: ' + str(gcj02_coords[0]))

            # 转换GCJ-02坐标到BD-09
            bd09_coords = gps_map.gcj02_to_bd09(gcj02_coords[0], gcj02_coords[1])
            print('BD-09 坐标: 经度: ' + str(bd09_coords[1]) + ' 纬度: ' + str(bd09_coords[0]))

            print("*********************")
except KeyboardInterrupt:
    ser.close()  # 关闭串口
    print("程序已结束")
